//
// Created by jg on 2021/1/4.
//

#ifndef FAST_MARCHING_FAST_MARCHING_H
#define FAST_MARCHING_FAST_MARCHING_H

#include <ros/ros.h>
#include <eigen3/Eigen/Dense>

#include <env_edt/plan_map.h>
#include <env_edt/utils.h>
#include <env_edt/corridor_generator.h>

#include <fast_methods/fm/solver.hpp>

class Path_FMM{
protected:
    MAP_EDT::Ptr map_edt_;
    shared_ptr<Solver<FMGrid3D>> fm_solver_;

    FMGrid3D grid_fmm_;
    double max_vel_, MAX_VEL_, MAX_ACC_;

    vector<Eigen::Vector3d> path_;
    Eigen::Vector3d start_pt_, end_pt_, start_vel_;

    vector<unsigned int> startIndices_;
    unsigned int goalIdx_;
public:
    Path_FMM(MAP_EDT::Ptr map_edt) : max_vel_(-1){
        map_edt_.swap(map_edt);
        fm_solver_ = std::make_shared<FMMStar<FMGrid3D>>("FMM*_Dist", TIME);
    }
    ~Path_FMM(){}

    void init(ros::NodeHandle& nh);

    void setFMMenv(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
    void getfMMPath(vector<Eigen::Vector3d>& path_coord, vector<double>& time);
    void getCorridor(vector<Cube>& corridor, vector<double>& time, Eigen::Vector3d vel);

    typedef shared_ptr<Path_FMM> Ptr;
protected:
    void sortPath(vector<Eigen::Vector3d>& path_coord, vector<double>& time);
    void timeAllocation(vector<Cube>& corridor, vector<double>& time);
};

#endif //FAST_MARCHING_FAST_MARCHING_H
